#!/usr/bin/env python
import roslib; roslib.load_manifest('ebotMain')
import rospy
import tf
import math
from std_msgs.msg import String, Float32
from nav_msgs.msg import Odometry
  
class odometryPublisher:
# Starts a node, odometry_publisher, which subscribes to the "robot_velocities" topic and calculates mileage from that. The odometry message (nav_msgs/Odometry) is published on the "odom" topic
	

	def __init__(self):	# Initialization method
		rospy.init_node('odometry_publisher')
		#print "'odometry_publisher' node initialized"
		self.odomPub = rospy.Publisher('odom', Odometry) 
		#print "created odomPub"
		self.tb = tf.TransformBroadcaster()
		#print "created tf broadcaster"

		print "Subscribes to : 'robot_vels' (string)"
		print "Publishes  to : 'odom'       (Odometry)"
		print "Broadcasts to : 'tf' "

		self.COV = 1e+100 # constant in covariance matrix for Odometry message
		self.lin_vel = 0
		self.ang_vel = 0
		self.dt = .5 # time increment in seconds. Change dt here, it should change all occurances in code
		self.x = 0; self.y = 0; self.th = 0 # default/starting x, y, and thetas
		

	def subscribe_speeds(self):
		rospy.Subscriber("robot_vels", String, self.publish_odom)
		rospy.spin()

	def publish_odom(self, data):

# Variables:
# lin_vel --> object field holding linear velocity retrieved from 'robot_vels' topic  
# ang_vel --> same as lin_vel, but for angular velocity
# avel --> varying angular velocity, used in calculating odometry
# lvel --> same as avel, but for linear velocity
# dt --> both an object field (self.dt) and a publish_odom() variable, dt. Holds the same value, really
			# retrieve lin, ang speeds from "robot_vels" topic
		speeds = data.data.split(' ')		
		self.lin_vel = float(speeds[0])
		self.ang_vel = float(speeds[1])
			     	
		#rospy.loginfo("I heard %s, %s", self.lin_vel, self.ang_vel )

			# initial x,y,th values
		x = self.x; y = self.y; th = self.th
		lvel = self.lin_vel; avel = self.ang_vel
		dt = self.dt

			# calc odom:
		x += dt*lvel*math.cos(th)
		y += dt*lvel*math.sin(th)
		th += dt*avel
		
		#rospy.loginfo("%s, %s, %s", x,y,th)
			# record temp values
		self.x = x; self.y = y; self.th = th

		# publishing odometry and transforms
		quat = tf.transformations.quaternion_from_euler(0,0,th)	
		
		o = Odometry()
		o.header.stamp = rospy.Time.now()
		o.header.frame_id = "base_link" 
		o.pose.pose.position.x = x
		o.pose.pose.position.y = y
		o.pose.pose.position.z = 0
		o.pose.pose.orientation.x = quat[0]		
		o.pose.pose.orientation.y = quat[1]
		o.pose.pose.orientation.z = quat[2]
		o.pose.pose.orientation.w = quat[3]	
		
		o.pose.covariance = [1e-5, 0, 0, 0, 0, 0,
					0, 1e-5, 0, 0, 0, 0, 
					0, 0, self.COV, 0, 0, 0,
					0, 0, 0, self.COV, 0, 0,
					0, 0, 0, 0, self.COV, 0,
					0, 0, 0, 0, 0, 1e-3]
		o.twist.twist.linear.x = lvel*math.cos(th)
		o.twist.twist.linear.y = lvel*math.sin(th)
		o.twist.twist.angular.z = avel # orig was x, but I'm using z. Right?
		o.twist.covariance = o.pose.covariance
	# do I need another publisher for this  message?
		#o_pose = PoseStamped()
		#o_pose.header.stamp = rospy.Time.now()
		#o_pose.header.frame = "odom_combined"
		#o_pose.pose.oriencation.x = quat[0]
		#o_pose.pose.oriencation.y = quat[1]
		#o_pose.pose.oriencation.z = quat[2]
		#o_pose.pose.oriencation.w = quat[3]
		#o_pose.pose.position=Point(0,0,0)

		self.odomPub.publish(o)
		self.tb.sendTransform((x,y,0), quat, rospy.Time.now(), 'base_link', 'odom')
	
if __name__ == '__main__':
	op = odometryPublisher()
	print "Creating new instance of 'odometryPublisher'"
	op.subscribe_speeds()
